g_enable_m = eye(2);
g_enable_m(2,2) = 0;
a_sj = newSingleJoint([pos_st 0 0 ;pos_st 0 0 ],g_enable_m);


%best for 2.379/0.009/30 , new best 3.444/0.011/50
kp = [3.444;1.769];
ki = [0.011;0.0049];
kd =[ 50;9.6];
%traj是三行，一行为一个量的目标值
% res = a_sj.pos_traj_control(kp,ki,kd,sample_period,simu_step,simu_period,value);
% none_res = a_none_g_sj.pos_traj_control(kp,ki,kd,sample_period,simu_step,simu_period,value);





%位置S仿真
pos_pid = pidControler(a_sj.n_group,kp,ki,kd);

state_info = zeros(a_sj.n_group,length(1:simu_step),3);
debug_info = zeros(a_sj.n_group,length(1:simu_step),4);

con_value = 0;
sample_time_count = 0;
for i = 1:simu_step
    state_info(:,i,:) = (a_sj.simu_loop(con_value,simu_period));
    sample_time_count = sample_time_count+simu_period;
    if(sample_time_count >= sample_period)
        pos_pid.set_target(pos_s_traj_value(i,1));
        debug_info(:,i,:) = pos_pid.get_sum_up_con_value(a_sj.joint_state(:,1));
        con_value = debug_info(:,i,1);
        sample_time_count = sample_time_count-sample_period;
    end

end


figure;
plot(0:simu_period:(simu_step-1)*simu_period,state_info(1,:,1),'-*','MarkerIndices',1:8000:simu_step,'MarkerSize',2);
hold on
plot(0:simu_period:(simu_step-1)*simu_period,state_info(2,:,1),'m-o','MarkerIndices',1:5000:simu_step,'MarkerSize',2);
hold on
plot(0:simu_period:(simu_step-1)*simu_period,pos_s_traj_value(1:end-1,1),'r--^','MarkerIndices',1:12000:simu_step,'MarkerSize',2);
legend('theta','none g','target')
title("pos S traj follow  theta");
figure;
plot(0:simu_period:(simu_step-1)*simu_period,state_info(1,:,2),'-*','MarkerIndices',1:8000:simu_step,'MarkerSize',2);
hold on
plot(0:simu_period:(simu_step-1)*simu_period,state_info(2,:,2),'m-o','MarkerIndices',1:5000:simu_step,'MarkerSize',2);
hold on
plot(0:simu_period:(simu_step-1)*simu_period,pos_s_traj_value(1:end-1,2),'r--^','MarkerIndices',1:12000:simu_step,'MarkerSize',2);
legend('omega','none g','target');
title("pos S traj follow  omega");
figure;
plot(0:simu_period:(simu_step-1)*simu_period,state_info(1,:,3),'-*','MarkerIndices',1:8000:simu_step,'MarkerSize',2);
hold on
plot(0:simu_period:(simu_step-1)*simu_period,state_info(2,:,3),'m-o','MarkerIndices',1:5000:simu_step,'MarkerSize',2);
hold on
plot(0:simu_period:(simu_step-1)*simu_period,pos_s_traj_value(1:end-1,3),'r--^','MarkerIndices',1:12000:simu_step,'MarkerSize',2);
legend('acc','none g','target');
ylim([-3 3]);
title("pos S traj follow  acc");